#include <inexbot.h>
#include <iostream>
#define EC_OUT 0x7011
#define EC_IN  0x6001

#define EC_A1_OUT 0x7012
#define EC_A2_OUT 0x7013

#define EC_A1_IN 0x6002
#define EC_A2_IN 0x6003

InexBotIO::InexBotIO() { }
InexBotIO::~InexBotIO() { }

void InexBotIO::Retrieve(uint8_t* domainPtr) {
    if (m_PdoOffset_.find(EC_IN) != m_PdoOffset_.end()) {
        txpdo_.dig = EC_READ_U16(domainPtr + *(m_PdoOffset_[EC_IN]));
    }

    if (m_PdoOffset_.find(EC_A1_IN) != m_PdoOffset_.end()) {
        txpdo_.ana[EC_A1_IN] = EC_READ_U16(domainPtr + *(m_PdoOffset_[EC_A1_IN]));
    }

    if (m_PdoOffset_.find(EC_A2_IN) != m_PdoOffset_.end()) {
        txpdo_.ana[EC_A2_IN] = EC_READ_U16(domainPtr + *(m_PdoOffset_[EC_A2_IN]));
    }
}

void InexBotIO::Publisher(uint8_t* domainPtr) {
    if (m_PdoOffset_.find(EC_OUT) != m_PdoOffset_.end()){
        EC_WRITE_U16(domainPtr + *(m_PdoOffset_[EC_OUT]), rxpdo_.pos);
    }

    if (m_PdoOffset_.find(EC_A1_OUT) != m_PdoOffset_.end()){
        EC_WRITE_U16(domainPtr + *(m_PdoOffset_[EC_A1_OUT]), 0xFFFF);
    }
}
